#! /usr/bin/env python3

import rospy

import tf2_ros
from geometry_msgs.msg import TransformStamped,Twist
import math

if __name__=="__main__":
    rospy.init_node("ctrol_vel_sub")

    buffer=tf2_ros.Buffer()
    bufferLister=tf2_ros.TransformListener(buffer)

    pub=rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)

    rate=rospy.Rate(10)

    while not rospy.is_shutdown():
        rate.sleep()
        try:
            trans = buffer.lookup_transform("turtle1", "turtle2", rospy.Time(0))
            twist = Twist()
            print("trans.x=%s, trans.y=%s", trans.transform.translation.x, trans.transform.translation.y)

            # 计算距离，以便于调整速度
            distance = math.sqrt(math.pow(trans.transform.translation.x, 2) + math.pow(trans.transform.translation.y, 2))
            twist.linear.x = 1.5 * distance if distance > 0.1 else 0  # 如果距离小于0.1则停止移动

            # 计算角度，以便于调整方向
            angle_to_target = math.atan2(trans.transform.translation.y, trans.transform.translation.x)
            twist.angular.z = 4 * angle_to_target

            print("x=%s, z=%s", twist.linear.x, twist.angular.z)
            pub.publish(twist)
            
        except Exception as e:
            rospy.logwarn("警告：%s",e)

    
    